#include <ros/ros.h>
#include <iostream>
#include <string>
#include "color_correction/CalibrateColorCorrection.h"
#include "color_correction/ApplyColorCorrection.h"
#include <locale>

class ColorCorrectionClient {
private:
    ros::NodeHandle nh_;
    ros::ServiceClient calibrate_client_;
    ros::ServiceClient apply_client_;
    
public:
    ColorCorrectionClient() : nh_() {
        // Initialize service clients
        calibrate_client_ = nh_.serviceClient<color_correction::CalibrateColorCorrection>("/color_correction/calibrate");
        apply_client_ = nh_.serviceClient<color_correction::ApplyColorCorrection>("/color_correction/apply_to_file");
        
        ROS_INFO("色彩校正客户端初始化完成");
    }
    
    bool calibrate(const std::string& reference_path, const std::string& input_path, const std::string& config_path) {
        ROS_INFO("正在调用校准服务...");
        ROS_INFO("参考图像: %s", reference_path.c_str());
        ROS_INFO("输入图像: %s", input_path.c_str());
        ROS_INFO("配置保存路径: %s", config_path.c_str());
        
        // Wait for service to be available
        if (!calibrate_client_.waitForExistence(ros::Duration(10.0))) {
            ROS_ERROR("校准服务不可用");
            return false;
        }
        
        color_correction::CalibrateColorCorrection srv;
        srv.request.reference_image_path = reference_path;
        srv.request.input_image_path = input_path;
        srv.request.config_save_path = config_path;
        
        if (calibrate_client_.call(srv)) {
            if (srv.response.success) {
                ROS_INFO("校准成功: %s", srv.response.message.c_str());
                return true;
            } else {
                ROS_ERROR("校准失败: %s", srv.response.message.c_str());
                return false;
            }
        } else {
            ROS_ERROR("调用校准服务失败");
            return false;
        }
    }
    
    bool apply(const std::string& input_path, const std::string& config_path, const std::string& output_path) {
        ROS_INFO("正在调用应用校正服务...");
        ROS_INFO("输入图像: %s", input_path.c_str());
        ROS_INFO("配置路径: %s", config_path.c_str());
        ROS_INFO("输出路径: %s", output_path.c_str());
        
        // Wait for service to be available
        if (!apply_client_.waitForExistence(ros::Duration(10.0))) {
            ROS_ERROR("应用校正服务不可用");
            return false;
        }
        
        color_correction::ApplyColorCorrection srv;
        srv.request.input_image_path = input_path;
        srv.request.config_path = config_path;
        srv.request.output_image_path = output_path;
        
        if (apply_client_.call(srv)) {
            if (srv.response.success) {
                ROS_INFO("色彩校正应用成功: %s", srv.response.message.c_str());
                return true;
            } else {
                ROS_ERROR("色彩校正失败: %s", srv.response.message.c_str());
                return false;
            }
        } else {
            ROS_ERROR("调用应用校正服务失败");
            return false;
        }
    }
};

void printUsage(const std::string& program_name) {
    std::cout << "颜色校正ROS客户端工具 (C++版本)" << std::endl;
    std::cout << "用法:" << std::endl;
    std::cout << "1. 校准模式:" << std::endl;
    std::cout << "   " << program_name << " calibrate <参考图像路径> <输入图像路径> <配置保存路径>" << std::endl;
    std::cout << "2. 应用模式:" << std::endl;
    std::cout << "   " << program_name << " apply <输入图像路径> <配置文件路径> <输出图像路径>" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << program_name << " calibrate reference.jpg input.jpg config.json" << std::endl;
    std::cout << "  " << program_name << " apply target.jpg config.json corrected.jpg" << std::endl;
}

int main(int argc, char** argv) {
    std::setlocale(LC_ALL, "zh_CN.UTF-8");
    ros::init(argc, argv, "color_correction_client");
    
    if (argc < 2) {
        printUsage(argv[0]);
        return 1;
    }
    
    std::string mode = argv[1];
    
    try {
        ColorCorrectionClient client;
        
        if (mode == "calibrate") {
            if (argc != 5) {
                std::cout << "校准模式需要4个参数: calibrate <参考图像路径> <输入图像路径> <配置保存路径>" << std::endl;
                return 1;
            }
            
            std::string reference_path = argv[2];
            std::string input_path = argv[3];
            std::string config_path = argv[4];
            
            bool success = client.calibrate(reference_path, input_path, config_path);
            return success ? 0 : 1;
            
        } else if (mode == "apply") {
            if (argc != 5) {
                std::cout << "应用模式需要4个参数: apply <输入图像路径> <配置文件路径> <输出图像路径>" << std::endl;
                return 1;
            }
            
            std::string input_path = argv[2];
            std::string config_path = argv[3];
            std::string output_path = argv[4];
            
            bool success = client.apply(input_path, config_path, output_path);
            return success ? 0 : 1;
            
        } else {
            std::cout << "未知模式: " << mode << std::endl;
            printUsage(argv[0]);
            return 1;
        }
        
    } catch (const std::exception& e) {
        ROS_ERROR("客户端错误: %s", e.what());
        return 1;
    }
    
    return 0;
}
